%
%***********************************************************************

% Date: 2013-05-26  Time: 22:02:34

%************************ CADAC UTILITIES ******************************
%***************************** UTL.FOR *********************************
%***********************************************************************
%***  *
%***  * MATxxx, VECxxx, TABLx,and CADxxx Utility Subroutines.
%***  *
%***  * MODIFICATION HISTORY
%***  * 970430 Modified VECANG to prevent rare math overflow problem, PZi
%***  * 970501 Version 2.0, Released with CADX2.FOR, Version 2
%***  * 970619 Corrections in MATADD and MATSUB, PZi
%***  * 980529 RANF Function added for RANF calls with FNGAUSS routine, bc
%***  * 991122 Added new table look-up TBLPx routines with pointers, PZi
%***  * 000204 Added subroutine US76 std atmosphere, PZi
%***  * 000412 Included CADxx utilities from UTL8.FOR, renamed UTL3.FOR, PZi
%***  *
%***  *****************************************************************



%***********************************************************************
%****************** VECTOR UTILITIES ***********************************
%***********************************************************************
;  j=n+1;  i=n+1;
function [z,x,y]=vecucr(z,x,y);

%*** PERFORMS UNIT CROSS PRODUCT OF 3-VECTORS
%     NAMELY: Z=UNIT(X cross Y)





z(1) = x(2).*y(3)-x(3).*y(2);
z(2) = x(3).*y(1)-x(1).*y(3);
z(3) = x(1).*y(2)-x(2).*y(1);
zm=fnrss(z,3);
z(1)=z(1)./zm;
z(2)=z(2)./zm;
z(3)=z(3)./zm;
return;
end %subroutine vecucr

function [d,a,b,c]=vecvec(d,a,b,c);

%*** CONSTRUCT VECTOR D FROM ELEMENTS A,B,C





d(1)=a;
d(2)=b;
d(3)=c;
return;
end %subroutine vecvec

function [d,a,b,c]=vecuvc(d,a,b,c);

%*** CONSTRUCT UNIT VECTOR FROM ELEMENTS A,B,C





[d,a,b,c]=vecvec(d,a,b,c);
rssd=fnrss(d,3);
d(1)=a./rssd;
d(2)=b./rssd;
d(3)=c./rssd;
return;
end %subroutine vecuvc

function [a,b,c]=vecang(a,b,c);

%*** ANGLE A(RAD) BETWEEN VECTOR B(3X1) AND C(3X1)





sca=b(1).*c(1)+b(2).*c(2)+b(3).*c(3);
bsq=b(1).*b(1)+b(2).*b(2)+b(3).*b(3);
csq=c(1).*c(1)+c(2).*c(2)+c(3).*c(3);
dumm=bsq.*csq;
%ZI970430      IF(DUMM.NE.0.) THEN
if(dumm > 1.0e-5)
dum=sca./sqrt(dumm);
else;
dum=1.;
end;
if(abs(dum) > 1.)
dum=(abs(1.).*sign(dum));
end;
a=acos(dum);
return;
end %subroutine vecang

function [fnrssresult,r,n]=fnrss(r,n);

%*** FUNCTION CONSTRUCTS ROOT-SUM-SQUARE OF N-VECTOR R




fnrssresult=[];

sum=0.;
for  i=1:n;
sum=sum+r(i).^2;
end;  i=n+1;
fnrssresult=sqrt(sum);
return;
end %function fnrss

%******************************************************************

function [fngausresult,xmean,sigma]=fngaus(xmean,sigma);

%*** GAUSSIAN DISTRIBUTION

fngausresult=[];

v1=ranf();
v2=ranf();
fngausresult=sigma.*sqrt(2..*log(1../v1)).*cos(6.2831853072.*v2)+xmean;
end %function fngaus

%**********************************************************************
%*** Random Number Function Generator                               ***

function [ranfresult]=ranf();
ranfresult=[];
rvalue=[];
;
rvalue =rand(size( rvalue ));
while( rvalue == 0.0 );
rvalue =rand(size( rvalue ));
end;
ranfresult = rvalue;
return;
end %function ranf
%***********************************************************************
%*************** TABLE LOOK-UP UTILITIES *******************************
%***********************************************************************

function [x,xi,yi,nx,y]=table(x,xi,yi,nx,y);





if(x < xi(1))
x=xi(1);
end;
if(x > xi(nx))
x=xi(nx);
end;
for  i = 2: nx;
if(x <= xi(i))
go to 20;
end;
end;  i = nx+1;
end;
end;
i = nx;
pct =(x - xi(i-1)) ./(xi(i) - xi(i-1));
end loop20;
y = yi(i-1) + pct .*(yi(i) - yi(i-1));
return;
end %subroutine table

function [x,y,xy,zi,nxy,z]=tabl2(x,y,xy,zi,nxy,z);


persistent t ; 

if isempty(t), t=zeros(1,2); end;


nx = nxy(1);
ny = nxy(2);
if(x < xy(1))
x=xy(1);
end;
if(x > xy(nx))
x=xy(nx);
end;
if(y < xy(nx+1))
y=xy(nx+1);
end;
if(y > xy(nx+ny))
y=xy(nx+ny);
end;
for  m = 2:ny;
j = nx + m;
if(y <= xy(j))
go to 20;
end;
end;  m =ny+1;
m = ny;
pcty =(y - xy(j-1)) ./(xy(j) - xy(j-1));
for  i = 2:nx;
if(x <= xy(i))
go to 40;
end;
end;  i =nx+1;
end;
i = nx;
pctx =(x - xy(i-1)) ./(xy(i) - xy(i-1));
for  k = 1:2;
ky = nx .*(m - k) + i;
t(k) = zi(ky-1) + pctx .*(zi(ky) - zi(ky-1));
end;  k =2+1;
z = t(2) + pcty .*(t(1) - t(2));
return;
end %subroutine tabl2

function [x,y,z,xyz,zw,nxyz,w]=tabl3(x,y,z,xyz,zw,nxyz,w);


persistent s t ; 

if isempty(t), t=zeros(2,2); end;
if isempty(s), s=zeros(1,2); end;


nx = nxyz(1);
ny = nxyz(2);
nz = nxyz(3);
if(x < xyz(1))
x=xyz(1);
end;
if(x > xyz(nx))
x=xyz(nx);
end;
if(y < xyz(nx+1))
y=xyz(nx+1);
end;
if(y > xyz(nx+ny))
y=xyz(nx+ny);
end;
if(z < xyz(nx+ny+1))
z=xyz(nx+ny+1);
end;
if(z > xyz(nx+ny+nz))
z=xyz(nx+ny+nz);
end;
for  i = 2:nx;
if(x <= xyz(i))
go to 20;
end;
end;  i =nx+1;
i = nx;
pctx =(x - xyz(i-1)) ./(xyz(i) - xyz(i-1));
for  j = 2:ny;
l = nx + j;
if(y <= xyz(l))
go to 40;
end;
end;  j =ny+1;
j = ny;
pcty =(y - xyz(l-1)) ./(xyz(l) - xyz(l-1));
nxy = nx + ny;
for  k = 2:nz;
m = nxy + k;
if(z <= xyz(m))
go to 60;
end;
end;  k =nz+1;
k = nz;
if((xyz(m)-xyz(m-1)) > 1.0e-10)
go to 65;
end;
pctz=0.0;
go to 67;
pctz =(z - xyz(m-1)) ./(xyz(m) - xyz(m-1));
nxny = nx .* ny;
for  jj = 1:2;
jz = nxny .*(k - jj);
for  kk = 1:2;
ky = jz + nx .*(j - kk) + i;
t(kk,jj) = zw(ky-1) + pctx .*(zw(ky) - zw(ky-1));
end;  kk =2+1;
end;  jj =2+1;
for  n = 1:2;
s(n) = t(2,n) + pcty .*(t(1,n) - t(2,n));
end;  n =2+1;
w = s(2) + pctz .*(s(1) - s(2));
return;
end %subroutine tabl3

function [x,y,z,w,xyzw,ww,nxyzw,ansmlv]=tabl4(x,y,z,w,xyzw,ww,nxyzw,ansmlv);


persistent r s t ; 

if isempty(t), t=zeros(2,2,2); end;
if isempty(r), r=zeros(2,2); end;
if isempty(s), s=zeros(1,2); end;


nx = nxyzw(1);
ny = nxyzw(2);
nz = nxyzw(3);
nw = nxyzw(4);
if(x < xyzw(1))
x=xyzw(1);
end;
if(x > xyzw(nx))
x=xyzw(nx);
end;
if(y < xyzw(nx+1))
y=xyzw(nx+1);
end;
if(y > xyzw(nx+ny))
y=xyzw(nx+ny);
end;
if(z < xyzw(nx+ny+1))
z=xyzw(nx+ny+1);
end;
if(z > xyzw(nx+ny+nz))
z=xyzw(nx+ny+nz);
end;
if(w < xyzw(nx+ny+nz+1))
w=xyzw(nx+ny+nz+1);
end;
if(w > xyzw(nx+ny+nz+nw))
w=xyzw(nx+ny+nz+nw);
end;
for  i = 2:nx;
if( x <= xyzw(i))
go to 20;
end;
end;  i =nx+1;
i = nx;
pctx =(x - xyzw(i-1)) ./(xyzw(i) - xyzw(i-1));
for  j = 2:ny;
l = nx + j;
if( y <= xyzw(l))
go to 40;
end;
end;  j =ny+1;
j = ny;
pcty =(y - xyzw(l-1)) ./(xyzw(l) - xyzw(l-1));
nxy = nx + ny;
for  k = 2:nz;
m = nxy + k;
if(z <= xyzw(m))
go to 60;
end;
end;  k =nz+1;
k = nz;
pctz =(z - xyzw(m-1)) ./(xyzw(m) - xyzw(m-1));
nxyz = nxy + nz;
for  l = 2:nw;
n = nxyz + l;
if(w <= xyzw(n))
go to 80;
end;
end;  l =nw+1;
l = nw;
pctw =(w - xyzw(n-1)) ./(xyzw(n) - xyzw(n-1));
nxny = nx .* ny;
nxnz = nxny .* nz;
for  ll = 1:2;
lll = nxnz .*(l-ll);
for  kk = 1:2;
kkk = lll + nxny .*(k-kk);
for  jj=1:2;
jjj = kkk + nx .*(j-jj) + i;
t(jj,kk,ll)=ww(jjj-1)+pctx.*(ww(jjj)-ww(jjj-1));
end;  jj=2+1;
end;  kk =2+1;
end;  ll =2+1;
for  lt = 1:2;
for  kt = 1:2;
r(kt,lt) = t(2,kt,lt) + pcty .*(t(1,kt,lt) - t(2,kt,lt));
end;  kt =2+1;
end;  lt =2+1;
for  n = 1:2;
s(n) = r(2,n) + pctz .*(r(1,n) - r(2,n));
end;  n =2+1;
ansmlv = s(2) + pctw .*(s(1) - s(2));
return;
end %subroutine tabl4

%***********************************************************************
%*************** TABLE LOOK-UP WITH POINTERS ***************************
%***********************************************************************

%#######################################################################

function [x,ix,xt,mx,yt,y,yd]=tblp1(x,ix,xt,mx,yt,y,yd);

%     VERSION 1.1   CONFIGURATION # 5.20
%#######################################################################

%     TBLP1 IS A GENERAL ONE-DIMENSIONAL TABLE LOOK-UP WITH POINTERS TO
%     ENHANCE SPEED


%     INPUTS:

%       X  - INDEPENDENT VARIABLE
%       IX - POINTER TO LAST PLACE IN TABLE WHERE LOOKUP OCCURED FOR X
%       XT - ARRAY OF INDEPENDENT VARIABLE VALUES (MX VALUES)
%       MX - MAXIMUN OF INDEPENDENT VARIABLE VALUES IN TABLE
%       YT - MATRIX OF DEPENDENT VARIABLES (MX VALUES)


%     OUTPUTS:

%       Y  - DEPENDENT VALUE FROM TABLE LOOK UP
%       YD - SLOPE OF Y WITH RESPECT TO THE INDEPENDENT VARIABLE

%#######################################################################



persistent ascnd ; 

if isempty(ascnd), ascnd=false; end;

%--FIND POINTER FOR INDEPENDENT VARIABLE

ascnd = xt(mx) >= xt(1);
ix = fix(max(1,min(ix,mx)));
if(x > xt(ix)==ascnd)

%--SEARCH UP FOR INDEPENDENT VARIABLE

for   i=ix+1:mx;
if(xt(i) > x==ascnd)
ix = fix(i-1);
go to 30;
end;
end;   i=mx+1;
ix = fix(mx-1);

%--SEARCH DOWN FOR INDEPENDENT VARIABLE

else;
for   i=ix-1:-1:1;
if(xt(i) <= x==ascnd)
ix = fix(i);
go to 30;
end;
end;   i=1-1;
ix = 1;
end;

%--NOW HAVE UPDATED POINTER -- DEFINE SOME VARIABLES FOR LATER USE

dtx = xt(ix)-xt(ix+1);
dx  = x-xt(ix);
%99      IF (DTX.EQ.0.0D0) THEN
%99      CALL ERROR(2010)
%99      ELSE

%--COMPUTE DERIVATIVE OF DATA WITH RESPECT TO INDEPENDENT VARIABLE

yd =(yt(ix)-yt(ix+1))./dtx;

%--INTERPOLATE

y = yt(ix) + yd.*dx;
%99      endif
return;
end %subroutine tblp1
%#######################################################################

function [x1,x2,ix1,ix2,xt1,xt2,mx1,mx2,yt,y,yd1,yd2]=tblp2(x1,x2,ix1,ix2,xt1,xt2,mx1,mx2,yt,y,yd1,yd2);

%     VERSION 1.1   CONFIGURATION # 5.21
%#######################################################################

%     TBLP2 IS A GENERAL TWO-DIMENSIONAL TABLE LOOK-UP WITH POINTERS TO
%     ENHANCE SPEED

%     INPUTS:

%       X1  - INDEPENDENT VARIABLE 1
%       X2  - INDEPENDENT VARIABLE 2
%       IX1 - POINTER TO LAST TABLE LOOKUP POSITION FOR X1
%       IX2 - POINTER TO LAST TABLE LOOKUP POSITION FOR X2
%       XT1 - ARRAY OF INDEPENDENT VARIABLE 1 VALUES (MX1 VALUES)
%       XT2 - ARRAY OF INDEPENDENT VARIABLE 2 VALUES (MX2 VALUES)
%       YT  - MATRIX OF DEPENDENT VARIABLES (MX1*MX2 VALUES)
%       MX1 - MAXIMUN OF INDEPENDENT VARIABLE 1 VALUES IN TABLE
%       MX2 - MAXIMUN OF INDEPENDENT VARIABLE 2 VALUES IN TABLE


%     OUTPUTS:

%       Y   - DEPENDENT VALUE FROM TABLE LOOK UP
%       YD1 - SLOPE OF Y WITH RESPECT TO INDEPENDENT VARIABLE 1
%       YD2 - SLOPE OF Y WITH RESPECT TO INDEPENDENT VARIABLE 2

%#######################################################################


persistent ascnd ; 

yt_orig=yt;yt_shape=[mx1,mx2];yt=reshape([yt_orig(1:min(prod(yt_shape),numel(yt_orig))),zeros(1,max(0,prod(yt_shape)-numel(yt_orig)))],yt_shape);

if isempty(ascnd), ascnd=false; end;

%--FIND POINTER FOR INDEPENDENT VARIABLE 1

ascnd = xt1(mx1) >= xt1(1);
ix1 = fix(max(1,min(ix1,mx1)));
if(x1 > xt1(ix1)==ascnd)

%--SEARCH UP FOR INDEPENDENT VARIABLE 1

for   i=ix1+1:mx1;
if(xt1(i) > x1==ascnd)
ix1 = fix(i-1);
go to 30;
end;
end;   i=mx1+1;
ix1 = fix(mx1-1);

%--SEARCH DOWN FOR INDEPENDENT VARIABLE 1

else;
for   i=ix1-1:-1:1;
if(xt1(i) <= x1==ascnd)
ix1 = fix(i);
go to 30;
end;
end;   i=1-1;
ix1 = 1;
end;

%--FIND POINTER FOR INDEPENDENT VARIABLE 2

ascnd = xt2(mx2) >= xt2(1);
ix2 = fix(max(1,min(ix2,mx2)));
if(x2 > xt2(ix2)==ascnd)

%--SEARCH UP FOR INDEPENDENT VARIABLE 2

for   i=ix2+1:mx2;
if(xt2(i) > x2==ascnd)
ix2 = fix(i-1);
go to 60;
end;
end;   i=mx2+1;
ix2 = fix(mx2-1);

%--SEARCH DOWN FOR INDEPENDENT VARIABLE 2

else;
for   i=ix2-1:-1:1;
if(xt2(i) <= x2==ascnd)
ix2 = fix(i);
go to 60;
end;
end;   i=1-1;
ix2 = 1;
end;

%--NOW HAVE UPDATED IX1 AND IX2 -- DEFINE SOME VARIABLES FOR LATER USE

dtx1 = xt1(ix1)-xt1(ix1+1);
dtx2 = xt2(ix2)-xt2(ix2+1);
%99      IF (DTX1.EQ.0.0D0.OR.DTX2.EQ.0.0D0) THEN
%99 CALL ERROR(2011)
%99      ELSE
if(dtx1 == 0.||dtx2 == 0.)
go to 70;
end;
dx1 = x1-xt1(ix1);
dx2 = x2-xt2(ix2);
s1  =(yt(ix1,ix2)-yt(ix1+1,ix2))./dtx1;
s3  =(yt(ix1,ix2+1)-yt(ix1+1,ix2+1))./dtx1;
y1  = yt(ix1,ix2)+s1.*dx1;
y3  = yt(ix1,ix2+1)+s3.*dx1;

%--COMPUTE DERIVATIVES OF DATA WITH RESPECT TO INDEPENDENT VARIABLES

yd2 =(y1-y3)./dtx2;

%--ERROR FIY MG 2-21-90

%        YD1 = (S3-S1)/DTX2*DX2+S1
yd1 =(s1-s3)./dtx2.*dx2+s1;

%--INTERPOLATE

y = y1+yd2.*dx2;
%99      endif
% 70 continue;
yt_orig(1:min(prod(yt_shape),numel(yt_orig)))=yt(1:min(prod(yt_shape),numel(yt_orig)));yt=yt_orig;
return;
end %subroutine tblp2
%#######################################################################

function [x1,x2,x3,ix1,ix2,ix3,xt1,xt2,xt3,mx1,mx2,mx3,yt,y,yd1,yd2,yd3]=tblp3(x1,x2,x3,ix1,ix2,ix3,xt1,xt2,xt3,mx1,mx2,mx3,yt,y,yd1,yd2,yd3);

%     VERSION 1.1   CONFIGURATION # 5.22
%#######################################################################

%     TBLP3 IS A GENERAL THREE-DIMENSIONAL TABLE LOOK-UP WITH POINTERS
%     TO ENHANCE SPEED


%     INPUTS:

%       X1  - INDEPENDENT VARIABLE 1
%       X2  - INDEPENDENT VARIABLE 2
%       X3  - INDEPENDENT VARIABLE 3
%       IX1 - POINTER TO LAST TABLE LOOKUP POSITION FOR X1
%       IX2 - POINTER TO LAST TABLE LOOKUP POSITION FOR X2
%       IX3 - POINTER TO LAST TABLE LOOKUP POSITION FOR X3
%       XT1 - ARRAY OF INDEPENDENT VARIABLE 1 VALUES (MX1 VALUES)
%       XT2 - ARRAY OF INDEPENDENT VARIABLE 2 VALUES (MX2 VALUES)
%       XT3 - ARRAY OF INDEPENDENT VARIABLE 3 VALUES (MX3 VALUES)
%       YT  - MATRIX OF DEPENDENT VARIABLES (MX1*MX2*MX3 VALUES)
%       MX1 - MAXIMUM OF INDEPENDENT VARIABLE 1 VALUES IN TABLE
%       MX2 - MAXIMUM OF INDEPENDENT VARIABLE 2 VALUES IN TABLE
%       MX3 - MAXIMUM OF INDEPENDENT VARIABLE 3 VALUES IN TABLE


%     OUTPUTS:

%       Y   - DEPENDENT VALUE FROM TABLE LOOK UP
%       YD1 - SLOPE OF Y WITH RESPECT TO INDEPENDENT VARIABLE 1
%       YD2 - SLOPE OF Y WITH RESPECT TO INDEPENDENT VARIABLE 2
%       YD3 - SLOPE OF Y WITH RESPECT TO INDEPENDENT VARIABLE 3

%#######################################################################


persistent ascnd ; 

yt_orig=yt;yt_shape=[mx1,mx2,mx3];yt=reshape([yt_orig(1:min(prod(yt_shape),numel(yt_orig))),zeros(1,max(0,prod(yt_shape)-numel(yt_orig)))],yt_shape);

if isempty(ascnd), ascnd=false; end;

%--FIND POINTER FOR INDEPENDENT VARIABLE 1

ascnd = xt1(mx1) >= xt1(1);
ix1 = fix(max(1,min(ix1,mx1)));
if(x1 > xt1(ix1)==ascnd)

%--SEARCH UP FOR INDEPENDENT VARIABLE 1

for   i=ix1+1:mx1;
if(xt1(i) > x1==ascnd)
ix1 = fix(i-1);
go to 30;
end;
end;   i=mx1+1;
ix1 = fix(mx1-1);

%--SEARCH DOWN FOR INDEPENDENT VARIABLE 1

else;
for   i=ix1-1:-1:1;
if(xt1(i) <= x1==ascnd)
ix1 = fix(i);
go to 30;
end;
end;   i=1-1;
ix1 = 1;
end;

%--FIND POINTER FOR INDEPENDENT VARIABLE 2

ascnd = xt2(mx2) >= xt2(1);
ix2 = fix(max(1,min(ix2,mx2)));
if(x2 > xt2(ix2)==ascnd)

%--SEARCH UP FOR INDEPENDENT VARIABLE 2

for   i=ix2+1:mx2;
if(xt2(i) > x2==ascnd)
ix2 = fix(i-1);
go to 60;
end;
end;   i=mx2+1;
ix2 = fix(mx2-1);

%--SEARCH DOWN FOR INDEPENDENT VARIABLE 2

else;
for   i=ix2-1:-1:1;
if(xt2(i) <= x2==ascnd)
ix2 = fix(i);
go to 60;
end;
end;   i=1-1;
ix2 = 1;
end;

%--FIND POINTER FOR INDEPENDENT VARIABLE 3

ascnd = xt3(mx3) >= xt3(1);
ix3 = fix(max(1,min(ix3,mx3)));
if(x3 > xt3(ix3)==ascnd)

%--SEARCH UP FOR INDEPENDENT VARIABLE 3

for   i=ix3+1:mx3;
if(xt3(i) > x3==ascnd)
ix3 = fix(i-1);
go to 90;
end;
end;   i=mx3+1;
ix3 = fix(mx3-1);

%--SEARCH DOWN FOR INDEPENDENT VARIABLE 3

else;
for   i=ix3-1:-1:1;
if(xt3(i) <= x3==ascnd)
ix3 = fix(i);
go to 90;
end;
end;   i=1-1;
ix3 = 1;
end;

%--NOW HAVE UPDATED IX1, IX2, AND IX3 -- DEFINE SOME VARIABLES FOR LATER
%--USE

dtx1 = xt1(ix1)-xt1(ix1+1);
dtx2 = xt2(ix2)-xt2(ix2+1);
dtx3 = xt3(ix3)-xt3(ix3+1);
%99      IF (DTX1.EQ.0.0D0.OR.DTX2.EQ.0.0D0.OR.DTX3.EQ.0.0D0) THEN
%99 CALL ERROR(2012)
%99      ELSE
dx1 = x1-xt1(ix1);
dx2 = x2-xt2(ix2);
dx3 = x3-xt3(ix3);
sz1 =(yt(ix1,ix2,ix3)    -yt(ix1,ix2,ix3+1))./dtx3;
sz2 =(yt(ix1+1,ix2,ix3)  -yt(ix1+1,ix2,ix3+1))./dtx3;
sz3 =(yt(ix1+1,ix2+1,ix3)-yt(ix1+1,ix2+1,ix3+1))./dtx3;
sz4 =(yt(ix1,ix2+1,ix3)  -yt(ix1,ix2+1,ix3+1))./dtx3;
sy1 =(yt(ix1,ix2,ix3)    -yt(ix1+1,ix2,ix3))./dtx1;
sy2 =(yt(ix1,ix2+1,ix3)  -yt(ix1+1,ix2+1,ix3))./dtx1;
sy3 =(yt(ix1,ix2+1,ix3+1)-yt(ix1+1,ix2+1,ix3+1))./dtx1;
sy4 =(yt(ix1,ix2,ix3+1)  -yt(ix1+1,ix2,ix3+1))./dtx1;
sx1 =(yt(ix1,ix2,ix3)    -yt(ix1,ix2+1,ix3))./dtx2;
sx2 =(yt(ix1+1,ix2,ix3)  -yt(ix1+1,ix2+1,ix3))./dtx2;
sx3 =(yt(ix1+1,ix2,ix3+1)-yt(ix1+1,ix2+1,ix3+1))./dtx2;
sx4 =(yt(ix1,ix2,ix3+1)  -yt(ix1,ix2+1,ix3+1))./dtx2;
z1t = yt(ix1,ix2,ix3)    +sz1.*dx3;
z2t = yt(ix1+1,ix2,ix3)  +sz2.*dx3;
z3t = yt(ix1+1,ix2+1,ix3)+sz3.*dx3;
z4t = yt(ix1,ix2+1,ix3)  +sz4.*dx3;
y1t = yt(ix1,ix2,ix3)    +sy1.*dx1;
y2t = yt(ix1,ix2+1,ix3)  +sy2.*dx1;
y3t = yt(ix1,ix2+1,ix3+1)+sy3.*dx1;
y4t = yt(ix1,ix2,ix3+1)  +sy4.*dx1;
x1t = yt(ix1,ix2,ix3)    +sx1.*dx2;
x2t = yt(ix1+1,ix2,ix3)  +sx2.*dx2;
x3t = yt(ix1+1,ix2,ix3+1)+sx3.*dx2;
x4t = yt(ix1,ix2,ix3+1)  +sx4.*dx2;
y1n = z1t+(z1t-z2t)./dtx1.*dx1;
y3n = z4t+(z4t-z3t)./dtx1.*dx1;
x1n = x1t+(x1t-x4t)./dtx3.*dx3;
x3n = x2t+(x2t-x3t)./dtx3.*dx3;
z1n = y1t+(y1t-y2t)./dtx2.*dx2;
z3n = y4t+(y4t-y3t)./dtx2.*dx2;
ss1 =(sz1-sz2)./dtx1;
ss3 =(sz4-sz3)./dtx1;
sy1 = sz1+ss1.*dx1;
sy3 = sz4+ss3.*dx1;

%--COMPUTE DERIVATIVES OF DATA WITH RESPECT TO INDEPENDENT VARIABLES

yd1 =(x1n-x3n)./dtx1;
yd2 =(y1n-y3n)./dtx2;
yd3 =(z1n-z3n)./dtx3;

%--INTERPOLATE

y = y1n+yd2.*dx2;
%99      endif
yt_orig(1:min(prod(yt_shape),numel(yt_orig)))=yt(1:min(prod(yt_shape),numel(yt_orig)));yt=yt_orig;
return;
end %subroutine tblp3
%#######################################################################

function [x1,x2,x3,x4,ix1,ix2,ix3,ix4,xt1,xt2,xt3,xt4,mx1,mx2,mx3,mx4,yt,y,sl1,sl2,sl3,sl4]=tblp4(x1,x2,x3,x4,ix1,ix2,ix3,ix4,xt1,xt2,xt3,xt4,mx1,mx2,mx3,mx4,yt,y,sl1,sl2,sl3,sl4);

%#######################################################################

%     TBLP4 IS A GENERAL FOUR-DIMENSIONAL TABLE LOOK-UP WITH POINTERS
%     TO ENHANCE SPEED


%     INPUTS:

%       X1  - INDEPENDENT VARIABLE 1
%       X2  - INDEPENDENT VARIABLE 2
%       X3  - INDEPENDENT VARIABLE 3
%       X4  - INDEPENDENT VARIABLE 4
%       IX1 - POINTER TO LAST TABLE LOOKUP POSITION FOR X1
%       IX2 - POINTER TO LAST TABLE LOOKUP POSITION FOR X2
%       IX3 - POINTER TO LAST TABLE LOOKUP POSITION FOR X3
%       IX4 - POINTER TO LAST TABLE LOOKUP POSITION FOR X4
%       XT1 - ARRAY OF INDEPENDENT VARIABLE 1 VALUES (MX1 VALUES)
%       XT2 - ARRAY OF INDEPENDENT VARIABLE 2 VALUES (MX2 VALUES)
%       XT3 - ARRAY OF INDEPENDENT VARIABLE 3 VALUES (MX3 VALUES)
%       XT4 - ARRAY OF INDEPENDENT VARIABLE 4 VALUES (MX4 VALUES)
%       YT  - MATRIX OF DEPENDENT VARIABLES (MX1*MX2*MX3*MX4 VALUES)
%       MX1 - MAXIMUM OF INDEPENDENT VARIABLE 1 VALUES IN TABLE
%       MX2 - MAXIMUM OF INDEPENDENT VARIABLE 2 VALUES IN TABLE
%       MX3 - MAXIMUM OF INDEPENDENT VARIABLE 3 VALUES IN TABLE
%       MX4 - MAXIMUM OF INDEPENDENT VARIABLE 4 VALUES IN TABLE


%     OUTPUTS:

%       Y - DEPENDENT VALUE FROM TABLE LOOK UP

%#######################################################################


persistent ascnd ; 
y4=[];dy1a=[];dy2a=[];dy3a=[];y4n=[];dy1b=[];dy2b=[];dy3b=[];
yt_orig=yt;yt_shape=[mx1,mx2,mx3,mx4];yt=reshape([yt_orig(1:min(prod(yt_shape),numel(yt_orig))),zeros(1,max(0,prod(yt_shape)-numel(yt_orig)))],yt_shape);

if isempty(ascnd), ascnd=false; end;

%--FIND POINTER FOR INDEPENDENT VARIABLE 4

ascnd = xt4(mx4) >= xt4(1);
ix4 = fix(max(1,min(ix4,mx4)));
if(x4 > xt4(ix4)==ascnd)

%--SEARCH UP FOR INDEPENDENT VARIABLE 4

for   i=ix4+1:mx4;
if(xt4(i) > x4==ascnd)
ix4 = fix(i-1);
go to 30;
end;
end;   i=mx4+1;
ix4 = fix(mx4-1);

%--SEARCH DOWN FOR INDEPENDENT VARIABLE 4

else;
for   i=ix4-1:-1:1;
if(xt4(i) <= x4==ascnd)
ix4 = fix(i);
go to 30;
end;
end;   i=1-1;
ix4 = 1;
end;

%--NOW HAVE UPDATED IX1, IX2, AND IX3 -- DEFINE SOME VARIABLES FOR LATER
%--USE

dtx4 = xt4(ix4+1)-xt4(ix4);
%99      IF (DTX4.EQ.0.0D0) THEN
%99        CALL ERROR(3012)
%99      ELSE
dx4  = x4-xt4(ix4);
[x1,x2,x3,ix1,ix2,ix3,xt1,xt2,xt3,mx1,mx2,mx3,yt(1:end,1:end,1:end,ix4:end),y4,dy1a,dy2a,dy3a]=tblp3(x1,x2,x3,ix1,ix2,ix3,xt1,xt2,xt3,mx1,mx2,mx3,yt(1:end,1:end,1:end,ix4:end),y4,dy1a,dy2a,dy3a);
[x1,x2,x3,ix1,ix2,ix3,xt1,xt2,xt3,mx1,mx2,mx3,yt(1:end,1:end,1:end,ix4+1:end),y4n,dy1b,dy2b,dy3b]=tblp3(x1,x2,x3,ix1,ix2,ix3,xt1,xt2,xt3,mx1,mx2,mx3,yt(1:end,1:end,1:end,ix4+1:end),y4n,dy1b,dy2b,dy3b);
yd =(y4n-y4)./dtx4;
y  = y4+yd.*dx4;
%99      endif

%--COMPUTE SLOPES

pct = dx4./dtx4;
sl1 = dy1a+pct.*(dy1b-dy1a);
sl2 = dy2a+pct.*(dy2b-dy2a);
sl3 = dy3a+pct.*(dy3b-dy3a);
sl4 = yd;
yt_orig(1:min(prod(yt_shape),numel(yt_orig)))=yt(1:min(prod(yt_shape),numel(yt_orig)));yt=yt_orig;
return;
end %subroutine tblp4

%************************************************************************
%************* doubleprecision MATRIX UTILITIES ************************
%************************************************************************

function [d,s,n1,n2]=dmatdbl(d,s,n1,n2);

%*** doubleprecision D(N1*N2) IS OBTAINED FROM SINGLE PRECISION S(N1*N2)



d_shape=size(d);d=reshape(d,1,[]);
s_shape=size(s);s=reshape(s,1,[]);



l=n1.*n2;
for  i=1:l;
d(i)=real(s(i));
end;  i=l+1;
d_shape=zeros(d_shape);d_shape(:)=d(1:numel(d_shape));d=d_shape;
s_shape=zeros(s_shape);s_shape(:)=s(1:numel(s_shape));s=s_shape;
return;
end %subroutine dmatdbl

function [s,d,n1,n2]=dmatsgl(s,d,n1,n2);

%*** SINGLE PRECISION S(N1*N2) IS OBTAINED FROM doubleprecision S(N1*N2)



s_shape=size(s);s=reshape(s,1,[]);
d_shape=size(d);d=reshape(d,1,[]);



l=n1.*n2;
for  i=1:l;
s(i)=(d(i));
end;  i=l+1;
s_shape=zeros(s_shape);s_shape(:)=s(1:numel(s_shape));s=s_shape;
d_shape=zeros(d_shape);d_shape(:)=d(1:numel(d_shape));d=d_shape;
return;
end %subroutine dmatsgl

function [c,a,b,n1,n2]=dmatadd(c,a,b,n1,n2);

%*** doubleprecision VERSION OF MATADD
%*** C(N1*N2) IS OBTAINED FROM ADDING A(N1*N2) TO B(N1*N2)




c_shape=size(c);c=reshape(c,1,[]);
a_shape=size(a);a=reshape(a,1,[]);
b_shape=size(b);b=reshape(b,1,[]);


for  j=1:n2;
for  i=1:n1;
k=n2.*(j-1)+i;
c(k)=a(k)+b(k);
end;  i=n1+1;
end;  j=n2+1;
c_shape=zeros(c_shape);c_shape(:)=c(1:numel(c_shape));c=c_shape;
a_shape=zeros(a_shape);a_shape(:)=a(1:numel(a_shape));a=a_shape;
b_shape=zeros(b_shape);b_shape(:)=b(1:numel(b_shape));b=b_shape;
return;
end %subroutine dmatadd

function [c,a,b,n1,n2]=dmatsub(c,a,b,n1,n2);

%---- Matlab: C = A - B

%*** doubleprecision VERSION OF MATSUB
%*** C(N1*N2) IS OBTAINED FROM SUBSTRACTING B(N1*N2) FROM A(N1*N2)




c_shape=size(c);c=reshape(c,1,[]);
a_shape=size(a);a=reshape(a,1,[]);
b_shape=size(b);b=reshape(b,1,[]);


for  j=1:n2;
for  i=1:n1;
k=n2.*(j-1)+i;
c(k)=a(k)-b(k);
end;  i=n1+1;
end;  j=n2+1;
c_shape=zeros(c_shape);c_shape(:)=c(1:numel(c_shape));c=c_shape;
a_shape=zeros(a_shape);a_shape(:)=a(1:numel(a_shape));a=a_shape;
b_shape=zeros(b_shape);b_shape(:)=b(1:numel(b_shape));b=b_shape;
return;
end %subroutine dmatsub




function [d,v,n]=dmatabs(d,v,n);

%--- Matlab: abs()??

%*** doubleprecision VERSION OF MATABS
%*** D IS THE ABSOLUTE VALUE OF VECTOR V(N*1)



persistent dv ; 

v_shape=size(v);v=reshape(v,1,[]);
if isempty(dv), dv=0; end;

dv=0.;
for  i=1:n;
dv=dv+v(i).*v(i);
end;  i=n+1;
d=sqrt(dv);
v_shape=zeros(v_shape);v_shape(:)=v(1:numel(v_shape));v=v_shape;
return;
end %subroutine dmatabs

function [c,a,b,n1,n2,n3]=dmatmul(c,a,b,n1,n2,n3);

%*** doubleprecision VERSION OF MATMUL
%*** C(N1*N2) IS OBTAINED FROM MULTIPLYING A(N1*N2) BY B(N2*N3)




c_shape=size(c);c=reshape(c,1,[]);
a_shape=size(a);a=reshape(a,1,[]);
b_shape=size(b);b=reshape(b,1,[]);


for i=1:n1;
for k=1:n3;
n=n1.*(k-1)+i;
c(n)=0.0;
for j=1:n2;
l=n1.*(j-1)+i;
m=n2.*(k-1)+j;
c(n)=c(n)+a(l).*b(m);
end; j=n2+1;
end; k=n3+1;
end; i=n1+1;
c_shape=zeros(c_shape);c_shape(:)=c(1:numel(c_shape));c=c_shape;
a_shape=zeros(a_shape);a_shape(:)=a(1:numel(a_shape));a=a_shape;
b_shape=zeros(b_shape);b_shape(:)=b(1:numel(b_shape));b=b_shape;
return;
end %subroutine dmatmul

function [c,a,n1,n2]=dmattra(c,a,n1,n2);

%--- Matlab: c = transpose(a)

%*** doubleprecision VERSION OF MATTRA
%*** C(N2*N1) IS THE TRANSPOSE OF A(N1*N2)




c_orig=c;c_shape=[n2,n1];c=reshape([c_orig(1:min(prod(c_shape),numel(c_orig))),zeros(1,max(0,prod(c_shape)-numel(c_orig)))],c_shape);
a_orig=a;a_shape=[n1,n2];a=reshape([a_orig(1:min(prod(a_shape),numel(a_orig))),zeros(1,max(0,prod(a_shape)-numel(a_orig)))],a_shape);


for  j=1:n2;
for  i=1:n1;
c(j,i)=a(i,j);
end;  i=n1+1;
end;  j=n2+1;
c_orig(1:min(prod(c_shape),numel(c_orig)))=c(1:min(prod(c_shape),numel(c_orig)));c=c_orig;
a_orig(1:min(prod(a_shape),numel(a_orig)))=a(1:min(prod(a_shape),numel(a_orig)));a=a_orig;
return;
end %subroutine dmattra

%          SUBROUTINE DMATINV

%          PURPOSE
%             COMPUTE THE INVERSE OF A NON-SINGLAR MATRIX
%             doubleprecision VERSION OF MATINV
%          USAGE
%             CALL DMATINV(AI,A,D,N)

%          DESCRIPTION OF PARAMETERS
%             A - NAME OF INPUT MATRIX
%             AI- NAME OF OUTPUT MATRIX (MAY BE THE SAME AS A)
%             D - DETERMINANT OF A
%             N - NUMBER OF ROWS AND COLUMNS OF A

%          REMARKS
%             ALL MATRICES ARE STORED AS GENERAL MATRICES

%        SET UP TO CALL GAUSS-JORDAN INVERSION ROUTINE.

%     ..................................................................

function [ai,a,d,n]=dmatinv(ai,a,d,n);

%-- Matlab: INV()

epsil=[];mx=[];m=[];ier=[];det=[];
ai_shape=size(ai);ai=reshape(ai,1,[]);
a_shape=size(a);a=reshape(a,1,[]);


%     SET UP TO CALL ROUTINE THAT ACTUALLY DOES THE CONVERSION

mx = n;
m = n;
epsil = 1.0d-10;

%         COPY A MATRIX INTO AI MATRIX

n2=n.*n;
for  i = 1: n2;
ai(i)=a(i);
end;  i = n2+1;

[epsil,mx,m,ai,ier,det]=dmatfs(epsil,mx,m,ai,ier,det);
d=det;
if(ier == 0)
ai_shape=zeros(ai_shape);ai_shape(:)=ai(1:numel(ai_shape));ai=ai_shape;
a_shape=zeros(a_shape);a_shape(:)=a(1:numel(a_shape));a=a_shape;
return;
end;
d=0.0d0;
writef(1,['%s '], ' MATRIX IS SINGULAR ');
x=-2;
y=sqrt(x).^2;
ai_shape=zeros(ai_shape);ai_shape(:)=ai(1:numel(ai_shape));ai=ai_shape;
a_shape=zeros(a_shape);a_shape(:)=a(1:numel(a_shape));a=a_shape;
return;
end %subroutine dmatinv

function [epsil,mx,m,a,ier,det]=dmatfs(epsil,mx,m,a,ier,det);

%------  Matlab function: INV()

%      DMATFS IS A DOUBLE-PRECISION MATRIX INVERSION SUBROUTINE
%     WRITTEN IN FORTRAN IV  - VARIABLE DIMENSIONING IS USED
%     OF MAX DIMENSIONS OF MATRIX IN CSLLING PROGRAM MUST BE SAME
%     VALUE AS PASSED ON BY CALLING SEQUENCE AS MX.
%     MATRIX CAN BE OF GENERAL FORM. METHOD IS GAUSS-JORDAN.


persistent i ibeta ifkp ig igkp iphi j jf k kp l lda temp ; 

format_1001=[' error','%u'];

a_orig=a;a_shape=[mx,mx];a=reshape([a_orig(1:min(prod(a_shape),numel(a_orig))),zeros(1,max(0,prod(a_shape)-numel(a_orig)))],a_shape);
if isempty(i), i=0; end;
if isempty(j), j=0; end;
if isempty(k), k=0; end;
if isempty(kp), kp=0; end;
if isempty(jf), jf=zeros(1,30); end;
if isempty(ibeta), ibeta=zeros(1,30); end;
if isempty(l), l=zeros(1,30); end;
if isempty(ig), ig=zeros(1,30); end;
if isempty(iphi), iphi=0; end;
if isempty(ifkp), ifkp=0; end;
if isempty(lda), lda=0; end;
if isempty(igkp), igkp=0; end;

if isempty(temp), temp=zeros(1,30); end;

%     INITIALIZE AND LOGICAL OPERATIONS ON COLUMN K, PIVOT COL.
det=1.0d0;
ier =0;
for  kp =1:m;
jf(kp) = 0;
ibeta(kp) = 0;
l(kp)=fix(kp);
ig(kp)=fix(kp);
end;  kp =fix(m+1);
%     TRANSFORM BY COLUMNS, K
for  k = 1: m;
%     FIND LARGEST ELEMENT IN COL K WITH IBETA STILL ZERO
rmu=0.0d0;
for  i = 1:m;
if(ibeta(i) < 0)
go to  1000;
elseif(ibeta(i) == 0) ;
go to    10;
else;
go to    14;
end;
if( abs(a(i,k)) -  abs(rmu) > 0.0)
go to    12;
else;
go to    14;
end;
%     LABEL ROW WITH LARGEST ELEMENT (RMU) AS IPHI
rmu = a(i,k);
iphi = fix(i);
end;  i =fix(m+1);
%     IF ELEMENT RMU IS TOO SMALL,MATRIX IS SINGULAR-CALL EXIT
if( abs(rmu) - epsil > 0.0)
go to    18;
end;
ier =1;
a_orig(1:min(prod(a_shape),numel(a_orig)))=a(1:min(prod(a_shape),numel(a_orig)));a=a_orig;
return;
det = rmu.*det;
%     SETF(K) WITH INDEX OF ROW, IBETA OF ROW NOW 1
jf(k) = fix(iphi);
ibeta(iphi) = 1;
%     ARITHMETIC OPERATIONS ON PIVOT COLUMN,K
%     FIRST SAVE ENTIRE COLUMN IN PRESENT FORM
for  i=1:m;
temp(i) = a(i,k);
end;  i=fix(m+1);
%     TRANSFORM COLUMN
a(iphi,k) = 1.0d0./rmu;
for  i=1:m;
if(i - iphi == 0)
go to    23;
end;
a(i,k) = -a(i,k)./rmu;
end;  i=fix(m+1);
%     LOGICAL OPERATIONS ON OTHER COLUMNS    PANEL IV
%     NOTE WE USE ORIG SAVED VALUES FROM PIVOT COLUMN,K
for  j =1:m;
%     TEST TO SKIP COL K, ALREADY TRANSFORMED
if(j-k == 0)
go to    31;
end;
%     NOT K   IF VALUE IN ROW IPHI IS 0 SKIP COLUMN
if(a(iphi,j) == 0.0)
go to    31;
end;
%     NOT ZERO
alpha = a(iphi,j);
a(iphi,j) = alpha./rmu;
%     TRANSFORM IN ONE COL,J, BY ROW INDICES
for  i =1:m;
%     TEST IF ELEMENT IS IN ROW WITH PIVOT ELEMENT
if(i-iphi == 0)
go to    30;
end;
%     NO
a(i,j) = a(i,j) - alpha.*temp(i) ./rmu;
%     YES
end;  i =fix(m+1);
end;  j =fix(m+1);
end;  k = fix(m+1);
%     PERMUTATIONS OF ROWS AND COLUMNS
mm=m-1;
for  kp=1:mm;
%     PERMUTATION OF ROWS
if(ig(kp)-jf(kp) == 0)
go to    55;
end;
ifkp = fix(jf(kp));
lda = fix(l(ifkp));
for  j =1:m;
temp(j) = a(kp,j);
a(kp,j) = a(lda,j);
a(lda,j) = temp(j);
det = -det;
end;  j =fix(m+1);
%     PERMUTATION OF COLUMNS
igkp = fix(ig(kp));
for  i = 1:m;
temp(i) = a(i,ifkp);
a(i,ifkp) = a(i,igkp);
a(i,igkp) = temp(i);
%     RESET INDICES
l(ifkp) = fix(k);
l(igkp) = fix(lda);
ig(lda) = fix(igkp);
ig(kp) = fix(ifkp);
end;  i =fix(m+1);
end;  kp=fix(mm+1);
a_orig(1:min(prod(a_shape),numel(a_orig)))=a(1:min(prod(a_shape),numel(a_orig)));a=a_orig;
return;
[writeErrFlag]=writeFmt(1,[format_1001]);
error(['stop encountered in original fortran code  ',char(10),';']);
%     INVERTED MATRIX IS IN CELLS ORIGINALLY OCCUPIED BY MATRIX
%     DETERMINANT IS IN CELL DET (NOT RETURNED)
a_orig(1:min(prod(a_shape),numel(a_orig)))=a(1:min(prod(a_shape),numel(a_orig)));a=a_orig;
return;
end %subroutine dmatfs
%**********************************************************************

function [rho,press,tempk,balt]=us76(rho,press,tempk,balt);
%**********************************************************************
%*** *
%*** * US Standard Atmosphere 1976
%*** *
%*** * This module performs the following functions:
%*** *  Calculates the atmospheric properties
%*** *
%*** * Argument Output:
%*** *     RHO=Air density - kg/m^3
%*** *     PRESS= Air static pressure - Pa
%*** *     TEMPK= Air temperature - degKelvin
%*** * Argument Input:
%*** *     BALT= Geometrical altitude above S.L. - m
%*** *
%*** * MODIFICATION HISTORY
%*** * 000204 Created by Peter Zipfel
%*** *
%*** ******************************************************************


persistent firstCall gmr gtab htab ntab pressl ptab rearth rhosl tempksl ttab ; if isempty(firstCall),firstCall=1;end; 

if isempty(rearth), rearth = 6369.0; end;
if isempty(gmr), gmr = 34.163195; end;
if isempty(rhosl), rhosl=1.22500; end;
if isempty(pressl), pressl=101325.; end;
if isempty(tempksl), tempksl=288.15; end;
if isempty(ntab), ntab=8; end;


if firstCall,   htab=[0.0,11.0,20.0,32.0,47.0,51.0,71.0,84.852];  end;
if firstCall,   ttab =[288.15,216.65,216.65,228.65,270.65,270.65,214.65,186.946];  end;
if firstCall,   ptab =[1.0,2.233611e-1,5.403295e-2,8.5666784e-3,1.0945601e-3,6.6063531e-4,3.9046834e-5,3.68501e-6];  end;
if firstCall,   gtab =[-6.5,0.0,1.0,2.8,0.0,-2.8,-2.0,0.0];  end;
firstCall=0;

alt=balt./1000.;
% convert geometric to geopotential altitude
h=alt.*rearth./(alt+rearth);

i=1;
% setting up for binary search
j=ntab;
while (1);
% integer division
k=fix((i+j)./2);
if(h < htab(k))
j=k;
else;
i=k;
end;
if(j <=(i+1))
break;
end;
end;

% i will be in 1...NTAB-1
tgrad=gtab(i);
tbase=ttab(i);
deltah=h-htab(i);
tlocal=tbase+tgrad.*deltah;
% temperature ratio
theta=tlocal./ttab(1);

% pressure ratio
if(tgrad == 0.0)
delta=ptab(i).*exp(-gmr.*deltah./tbase);
else;
delta=ptab(i).*(tbase./tlocal).^(gmr./tgrad);
end;

% density ratio
sigma=delta./theta;

rho=rhosl.*sigma;
press=pressl.*delta;
tempk=tempksl.*theta;
return;
end %subroutine us76

%***********************************************************************
%************************ CAD UTILITIES ********************************
%***********************************************************************

function [ama,ami,phi,aa]=cadelp(ama,ami,phi,aa);

%*** MAJOR, MINOR AXES OF ELLIPSE AND ROTATION ANGLE
%    FROM THE SYMMETRICAL POS SEMI-DEFINITE AA(2X2) MATRIX

%      Coordinate axes orientation:

%          ^ 1-axis
%          |
%          |---> 2-axis

%  ARGUMENT OUTPUT:

%          AMA   =Major semi-axis
%          AMI   =Minor semi=axis
%          PHIEV =Angle of major axis wrt first coord axis

%  ARGUMENT INPUT:

%          AA(2x2) =Real symmetrical pos semi-definite matrix



persistent x1v x2v ; 

aa_orig=aa;aa_shape=[2,2];aa=reshape([aa_orig(1:min(prod(aa_shape),numel(aa_orig))),zeros(1,max(0,prod(aa_shape)-numel(aa_orig)))],aa_shape);
if isempty(x1v), x1v=zeros(1,2); end;
if isempty(x2v), x2v=zeros(1,2); end;

%*** MAJOR AND MINOR SEMI-AXES (EIGENVALUES OF SUBMATRIX)

a11=aa(1,1);
a22=aa(2,2);
a12=aa(1,2);
a1133=a11+a22;
aq1122=a1133.^2;
dum1=aq1122-4..*(a11.*a22-a12.^2);
if(dum1 >= 0.)
dum2=sqrt(dum1);
end;
ama=(a1133+dum2)./2.;
ami=(a1133-dum2)./2.;
if(ama == ami)
aa_orig(1:min(prod(aa_shape),numel(aa_orig)))=aa(1:min(prod(aa_shape),numel(aa_orig)));aa=aa_orig;
return;
end;

%*** PRINCIPAL AXES IN ORIGINAL(V)-COORDINATES (EIGENVECTORS)

if(a11-ama ~= 0.)
dum1=-a12./(a11-ama);
ak1=sqrt(1../(1.+dum1.^2));
x1v(1)=dum1.*ak1;
x1v(2)=ak1;
dum=dum1.*ak1;
if(abs(dum) > 1.)
dum=(abs(1.).*sign(dum));
end;
phi=acos(dum);
else;
dum1=-a12./(a22-ama);
ak1=sqrt(1../(1.+dum1.^2));
x1v(1)=ak1;
x1v(2)=dum1.*ak1;
if(abs(ak1) > 1.)
ak1=(abs(1.).*sign(ak1));
end;
phi=acos(ak1);
end;
if(a11-ami ~= 0.)
dum2=-a12./(a11-ami);
ak2=sqrt(1../(1.+dum2.^2));
x2v(1)=dum2.*ak2;
x2v(2)=ak2;
else;
dum2=-a12./(a22-ami);
ak2=sqrt(1../(1.+dum2.^2));
x2v(1)=ak2;
x2v(2)=dum2.*ak2;
end;

aa_orig(1:min(prod(aa_shape),numel(aa_orig)))=aa(1:min(prod(aa_shape),numel(aa_orig)));aa=aa_orig;
return;
end %subroutine cadelp
%**********************************************************************

function [e,de,perg,ainc,antii,alngn,omeg,anu,altii, stii,vtii]=cadorb(e,de,perg,ainc,antii,alngn,omeg,anu,altii, stii,vtii);
%**********************************************************************
%*** *
%*** * Orbital elements from inerftial position and velocity
%*** *
%*** * Argument Output:
%*** *          E(3) =Orbit eccentricity vector - ND
%*** *          DE =Orbit eccentricity - ND
%*** *          PERG =Radius of perigee - m
%*** *          AINC =Inclination of orbit - rad
%*** *          ANTII(3) =Node vector - m^2/s
%*** *          ALNGN =Longitude of the ascending node - rad
%*** *          OMEG =Argument of periapsis - rad
%*** *          ANU =truemlv anomaly - rad
%*** *          ALTII(3) =Angular momentum vector of orbit - m^2/s
%*** * Argument Input:
%*** *          STII(3) =Position of T wrt I in inertial coord - m
%*** *          VTII(3) =Inertial Velocity of T in inertial coor - m/s
%*** *
%*** * MODIFICATION HISTORY
%*** * 840331 Created by Jim Williford
%*** *
%**********************************************************************


persistent arg2d dantid ded domegd dum2d dumv1 dumv2 dumv3 sstii ; 
dalti=[];danti=[];dti=[];dum1=[];dvti=[];dum2=[];dum3=[];dum4=[];
if isempty(dantid), dantid=0; end;
if isempty(ded), ded=0; end;
if isempty(dum2d), dum2d=0; end;
if isempty(arg2d), arg2d=0; end;
if isempty(domegd), domegd=0; end;
global bcom_1; if isempty(bcom_1), bcom_1=zeros(1,3510); end;
%% common c(3510);
%% common bcom_1(3510);

if isempty(sstii), sstii=zeros(3,3); end;
if isempty(dumv1), dumv1=zeros(1,3); end;
if isempty(dumv2), dumv2=zeros(1,3); end;
if isempty(dumv3), dumv3=zeros(1,3); end;

%*** INPUT FROM OTHER MODULES

% equivalence(bcom_1(0051),rearth) ::;
% equivalence(bcom_1(0052),crad) ::;
% equivalence(bcom_1(0057),amu) ::;

%*** TARGET ANGULAR MOMENTUM [ALTII] AND NODE [ANTII] VECTORS

[sstii,stii]=matsks(sstii,stii);
[altii,sstii,vtii]=matmul(altii,sstii,vtii,3,3,1);
[dalti,altii]=matabs(dalti,altii,3);

antii(1)=-altii(2);
antii(2)=altii(1);
antii(3)=0.;
[danti,antii]=matabs(danti,antii,3);

%*** ORBIT ECCENTRICITY

[dti,stii]=matabs(dti,stii,3);
[dum1,stii,vtii]=matsca(dum1,stii,vtii,3);
[dumv1,dum1,vtii]=matcon(dumv1,dum1,vtii,3,1);
[dvti,vtii]=matabs(dvti,vtii,3);
dum2=dvti.^2.-amu./dti;
[dumv2,dum2,stii]=matcon(dumv2,dum2,stii,3,1);
[dumv3,dumv2,dumv1]=matsub(dumv3,dumv2,dumv1,3,1);
dum2=1../amu;
%!
[e,dum2,dumv3]=matcon(e,dum2,dumv3,3,1);
[de,e]=matabs(de,e,3);

%*** TARGET PERIGEE (ABOVE GROUND), ORBIT INCLINATION, AND LONGITUDE
%    OF ASCENDING NODE

alatrc=(dalti.^2.)./amu;
peri=alatrc./(1+de);
perg=peri-rearth;

ainc=acos(altii(3)./dalti);

dalngn=acos(antii(1)./(sqrt(antii(1).^2.+antii(2).^2.)));

%*** ARGUMENT OF THE PERIGEE AND truemlv ANOMALY

[dum2,antii,e]=matsca(dum2,antii,e,3);
%*** CONVERSION TO doubleprecision
dantid=real(danti);
ded=real(de);
dum2d=real(dum2);

arg2d=dum2d./(dantid.*ded);
if(arg2d > 1.)
arg2d=1.;
end;
if(arg2d < -1.)
arg2d=-1.;
end;
domegd=acos(arg2d);
%*** CONVERSION TO SINGLE PRECISION
domeg=(domegd);

[dum3,e,stii]=matsca(dum3,e,stii,3);
arg=dum3./(de.*dti);
if(arg > 1.)
arg=1.;
end;
if(arg < -1.)
arg=-1.;
end;
danu=acos(arg);


%*** QUADRANT ACCOUNTING

twopi=360../crad;
[dum4,stii,vtii]=matsca(dum4,stii,vtii,3);

if(dum4 >= 0.)
anu=danu;
else;
anu=twopi-danu;
end;

if(e(3) >= 0.)
omeg=domeg;
else;
omeg=twopi-domeg;
end;

if(antii(2) > 0.)
alngn=dalngn;
else;
alngn=twopi-dalngn;
end;




return;
end %subroutine cadorb
%******************************************************************

function [tgi,blon,blat,balt,alon0]=cadtgi84(tgi,blon,blat,balt,alon0);
%******************************************************************
%*** * Calculates geographic (geocentric) wrt inertial T.M.
%*** *  using the WGS 84 reference ellipsoid
%*** * Reference: Britting,K.R.'Inertial Navigation Systems Analysis',
%*** * Wiley, 1971
%*** *
%*** * Argument ouput: TGI(3x3) =Geographic wrt inertial T.M.- ND
%*** * Argument input: BLON =Geodetic longitude - rad
%*** *                 BLAT =Geodetic latitude - rad
%*** *                 BALT =Height above ellipse - m
%*** *                 ALON0=Greenwich celestial longitude at t=0 - rad
%*** *
%*** * MODIFICATION HISTORY
%*** * 990413 Created by Peter Zipfel
%*** * 990625 Use of deflection DD instead of D0, PZi
%*** *
%*** **************************************************************

persistent ael ee tdi tgd ; 

tgi_orig=tgi;tgi_shape=[3,3];tgi=reshape([tgi_orig(1:min(prod(tgi_shape),numel(tgi_orig))),zeros(1,max(0,prod(tgi_shape)-numel(tgi_orig)))],tgi_shape);
global bcom_1; if isempty(bcom_1), bcom_1=zeros(1,3510); end;
%% common c(3510);
%% common bcom_1(3510);

if isempty(tgd), tgd=zeros(3,3); end;
if isempty(tdi), tdi=zeros(3,3); end;

%*** INPUT FROM EXEC

% equivalence(bcom_1(0058),weii3) ::;
% equivalence(bcom_1(2000),t) ::;

% WEII3 =Earth's mean angular velocity (WGS84) - rad/s

%*** WGS 84 Parameters

if isempty(ee), ee = 0.003352811; end;
if isempty(ael), ael = 6378137.; end;

% EE =flattening of earth's ellipsoid - ND
% AEL =Major axis of earth's ellipsoid - m

%*** Celestial longitude of vehicle at time T

blonc=alon0+weii3.*t+blon;

%*** T.M. of geodetic coord wrt inertial coord., TDI(3x3)

tdi(1,3)=cos(blat);
tdi(3,3)=-sin(blat);
tdi(2,2)=cos(blonc);
tdi(2,1)=-sin(blonc);
tdi(1,1)=tdi(3,3).*tdi(2,2);
tdi(1,2)=-tdi(3,3).*tdi(2,1);
tdi(3,1)=-tdi(1,3).*tdi(2,2);
tdi(3,2)=tdi(1,3).*tdi(2,1);
tdi(2,3)=0.;

%*** T.M. of geographic (geocentric) wrt geodetic coord., TGD(3x3)

[tgd]=matuni(tgd,3);
%Eq 4-21
r0=ael.*(1.-ee.*(1.-cos(2..*blat))./2.+5..*ee.^2.*(1.-cos(4..*blat))./16.);
%Eq 4-15
dd=ee.*sin(2..*blat).*(1.-ee./2.-balt./r0);
tgd(1,1)=cos(dd);
tgd(3,3)=tgd(1,1);
tgd(3,1)=sin(dd);
tgd(1,3)=-tgd(3,1);

%*** T.M. of geographic (geocentric) wrt inertial coord., TGI(3x3)

[tgi,tgd,tdi]=matmul(tgi,tgd,tdi,3,3,3);



tgi_orig(1:min(prod(tgi_shape),numel(tgi_orig)))=tgi(1:min(prod(tgi_shape),numel(tgi_orig)));tgi=tgi_orig;
return;
end %subroutine cadtgi84
%******************************************************************

function [blon,blat,balt,sbii,alon0]=cadgeo84(blon,blat,balt,sbii,alon0);
%******************************************************************
%*** * Calculates geodetic longitude, latitude, and altitude
%*** *  using the WGS 84 reference ellipsoid
%*** * Reference: Britting,K.R.'Inertial Navigation Systems Analysis',
%*** * Wiley, 1971
%*** *
%*** * Argument output: BLON =Geodetic longitude - rad
%*** *                  BLAT =Geodetic latitude - rad
%*** *      BALT =Altitude above ellipsoid - m
%*** * Argument input:  SBII(3x1) =Inertial vehicle position - m
%*** *                  ALON0=Greenwich celestial longitude at t=0 - rad
%*** *
%*** *
%*** * MODIFICATION HISTORY
%*** * 990414 Created by Peter Zipfel
%*** * 990625 Use of deflection DD instead of D0, PZi
%*** **************************************************************


persistent ael ee ; 
dbi=[];
global bcom_1; if isempty(bcom_1), bcom_1=zeros(1,3510); end;
%% common c(3510);
%% common bcom_1(3510);



%*** INPUT FROM EXEC

% equivalence(bcom_1(0058),weii3) ::;
% equivalence(bcom_1(2000),t) ::;

% WEII3 =Earth's mean angular velocity (WGS84) - rad/s

%*** WGS 84 Parameters

if isempty(ee), ee = 0.003352811; end;
if isempty(ael), ael = 6378137.; end;

% EE =flattening of earth's ellipsoid - ND
% AEL =Major axis of earth's ellipsoid - m

%*** Initialize geodetic latitude using geocentric latitude

[dbi,sbii]=matabs(dbi,sbii,3);
blatg=asin(sbii(3)./dbi);
blat=blatg;
icount=0;

%*** Iterative loop to caculate geodetic latitude and altitude

% 10 continue;

blm=blat;
%Eq 4-21
r0=ael.*(1.-ee.*(1.-cos(2..*blm))./2.+5..*ee.^2.*(1.-cos(4..*blm))./16.);
balt=dbi-r0;
%Eq 4-15
dd=ee.*sin(2..*blm).*(1.-ee./2.-balt./r0);
blat=blatg+dd;
icount=icount+1;
if(icount > 500)
%---  TYPE*,' *** STOP: 500 iterations in sbr CADGEO84 ***'
writef(1,['%s '], ' *** STOP: 500 iterations in sbr CADGEO84 ***');
error(['stop encountered in original fortran code  ',char(10),';']);
end;

if(abs(blat-blm) > 1e-7)
go to 10;
end;

%*** Geodetic (identical to geographic(geocentric)) longitude

blon=asin(sbii(2)./sqrt(sbii(1).^2+sbii(2).^2))-weii3.*t-alon0;



return;
end %subroutine cadgeo84


%******************************************************************

function [sbii,blon,blat,balt,alon0]=cadine84(sbii,blon,blat,balt,alon0);
%******************************************************************
%*** * Calculates inertial position vector from geodetic longitude,
%*** *  latitude, and altitude using the WGS 84 reference ellipsoid
%*** * Reference: Britting,K.R.'Inertial Navigation Systems Analysis'
%*** * pp.45-49, Wiley, 1971
%*** *
%*** * Argument ouput: SBII(3x1) =Inertial vehicle position - m
%*** * Argument input: BLON =Geodetic longitude - rad
%*** *                 BLAT =Geodetic latitude - rad
%*** *                 BALT =Altitude above ellipsoid - m
%*** *                 ALON0=Greenwich celestial longitude at t=0 - rad
%*** *
%*** * MODIFICATION HISTORY
%*** * 990413 Created by Peter Zipfel
%*** * 990625 Use of deflection DD instead of D0, PZi
%*** *
%*** **************************************************************


persistent ael ee sbid ; 

global bcom_1; if isempty(bcom_1), bcom_1=zeros(1,3510); end;
%% common c(3510);
%% common bcom_1(3510);

if isempty(sbid), sbid=zeros(1,3); end;

%*** INPUT FROM EXEC

% equivalence(bcom_1(0058),weii3) ::;
% equivalence(bcom_1(2000),t) ::;

% WEII3 =Earth's mean angular velocity (WGS84) - rad/s

%*** WGS 84 Parameters

if isempty(ee), ee = 0.003352811; end;
if isempty(ael), ael = 6378137.; end;

% EE =flattening of earth's ellipsoid - ND
% AEL =Major axis of earth's ellipsoid - m

%*** Deflection of the normal, DD, and length of earth's radius to ellipse, R0

%Eq 4-21
r0=ael.*(1.-ee.*(1.-cos(2..*blat))./2.+5..*ee.^2.*(1.-cos(4..*blat))./16.);
%Eq 4-15
dd=ee.*sin(2..*blat).*(1.-ee./2.-balt./r0);

%*** Vehicles's displacement vector from earth's center SBID(3x1) in geodetic coord.

dbi=r0+balt;
sbid(1)=-dbi.*sin(dd);
sbid(2)=0.;
sbid(3)=-dbi.*cos(dd);

%*** Celestial longitude of vehicle at time T

blonc=alon0+weii3.*t+blon;

%*** Vehicle's displacement vector in inertial coord. SBII(3x1)=TID(3x3)xSBID(3x3)

slat=sin(blat);
clat=cos(blat);
slon=sin(blonc);
clon=cos(blonc);
sbii(1)=-slat.*clon.*sbid(1)-clat.*clon.*sbid(3);
sbii(2)=-slat.*slon.*sbid(1)-clat.*slon.*sbid(3);
sbii(3)=clat.*sbid(1)-slat.*sbid(3);



return;
end %subroutine cadine84
%**********************************************************************

function [blon,blat,balt,sbii,alon0]=cadsph(blon,blat,balt,sbii,alon0);

%*** CALCULATES LON,LAT,ALT FROM INERTIAL POSITION VECTOR



global bcom_1; if isempty(bcom_1), bcom_1=zeros(1,3510); end;
%% common  c(3510);
%% common  bcom_1(3510);



%*** INPUT FROM EXEC

% equivalence(bcom_1(0051),rearth) ::;
% equivalence(bcom_1(0052),crad) ::;
% equivalence(bcom_1(0058),weii3) ::;
% equivalence(bcom_1(2000),t) ::;

%*** LATITUDE

dbii=sqrt(sbii(1).^2+sbii(2).^2+sbii(3).^2);
dum1=sbii(3)./dbii;
blat=asin(dum1);
balt=dbii-rearth;

%*** LONGITUDE

dum3=sbii(2)./sqrt(sbii(1).^2+sbii(2).^2);
dum4=asin(dum3);
if(sbii(1) >= 0.&&sbii(2) >= 0.)
alamda=dum4;
end;
if(sbii(1) < 0.&&sbii(2) >= 0.)
alamda=180../crad-dum4;
end;
if(sbii(1) < 0.&&sbii(2) < 0.)
alamda=180../crad-dum4;
end;
if(sbii(1) > 0.&&sbii(2) < 0.)
alamda=360../crad+dum4;
end;
blon=alon0+alamda-weii3.*t;
if(blon >(180../crad))
blon=-((360../crad)-blon);
end;





return;
end %subroutine cadsph
%**********************************************************************

function [tip,ainc,omega,along]=cadip(tip,ainc,omega,along);

%*** THIS SUBROUTINE COMPUTES THE TRANSFORMATION OF INERTIAL RELATIVE TO
%*** PERIFOCAL COORDINATE SYSTEMS



tip_orig=tip;tip_shape=[3,3];tip=reshape([tip_orig(1:min(prod(tip_shape),numel(tip_orig))),zeros(1,max(0,prod(tip_shape)-numel(tip_orig)))],tip_shape);
global bcom_1; if isempty(bcom_1), bcom_1=zeros(1,3510); end;
%% common c(3510);
%% common bcom_1(3510);



calong=cos(along);
comega=cos(omega);
cainc=cos(ainc);
salong=sin(along);
somega=sin(omega);
sainc=sin(ainc);

tip(1,1)=calong.*comega-salong.*somega.*cainc;
tip(1,2)=-calong.*somega-salong.*comega.*cainc;
tip(1,3)=salong.*sainc;

tip(2,1)=salong.*comega+calong.*somega.*cainc;
tip(2,2)=-salong.*somega+calong.*comega.*cainc;
tip(2,3)=-calong.*sainc;

tip(3,1)=somega.*sainc;
tip(3,2)=comega.*sainc;
tip(3,3)=cainc;

tip_orig(1:min(prod(tip_shape),numel(tip_orig)))=tip(1:min(prod(tip_shape),numel(tip_orig)));tip=tip_orig;
return;
end %subroutine cadip
%**********************************************************************

function [tpt,anu,decc]=cadpt(tpt,anu,decc);

%*** THIS SUBROUTINE COMPUTES THE TRANSFORMATION OF PERIFOCAL RELATIVE
%*** TO TARGET CENTERED COORDINATE SYSTEM




tpt_orig=tpt;tpt_shape=[3,3];tpt=reshape([tpt_orig(1:min(prod(tpt_shape),numel(tpt_orig))),zeros(1,max(0,prod(tpt_shape)-numel(tpt_orig)))],tpt_shape);


dum4=sin(anu);
dum5=cos(anu)+decc;
phio=atan2(dum4,dum5);

tpt(1,1)=-sin(phio);
tpt(1,2)=0.;
tpt(1,3)=-cos(phio);

tpt(2,1)=-tpt(1,3);
tpt(2,2)=0.;
tpt(2,3)=tpt(1,1);

tpt(3,1)=0.;
tpt(3,2)=-1.;
tpt(3,3)=0.;

tpt_orig(1:min(prod(tpt_shape),numel(tpt_orig)))=tpt(1:min(prod(tpt_shape),numel(tpt_orig)));tpt=tpt_orig;
return;
end %subroutine cadpt
%**********************************************************************

function [tli,st1ii,vt1ii]=cadli(tli,st1ii,vt1ii);

%*** TRANSFORMATION MATRIX OF LOCAL VERTICAL WRT. INERTIAL COOR.SYS.

%    LOCAL VERTICAL COOR.SYS.: X-> DIRECTION OF RADIUS VECTOR
%          Z-> DIRECTION OF ANGULAR MOM.VECTOR
%          Y-> COMPLETES RIGHT HANDED COOR.SYS.

dt1ii=[];dvt1ii=[];
tli_orig=tli;tli_shape=[3,3];tli=reshape([tli_orig(1:min(prod(tli_shape),numel(tli_orig))),zeros(1,max(0,prod(tli_shape)-numel(tli_orig)))],tli_shape);
global bcom_1; if isempty(bcom_1), bcom_1=zeros(1,3510); end;
%% common c(3510);
%% common bcom_1(3510);



[dt1ii,st1ii]=matabs(dt1ii,st1ii,3);
[dvt1ii,vt1ii]=matabs(dvt1ii,vt1ii,3);
al1i1=st1ii(1)./dt1ii;
al1i2=st1ii(2)./dt1ii;
al1i3=st1ii(3)./dt1ii;
dum=dt1ii.*dvt1ii;
al3i1=(st1ii(2).*vt1ii(3)-st1ii(3).*vt1ii(2))./dum;
al3i2=(st1ii(3).*vt1ii(1)-st1ii(1).*vt1ii(3))./dum;
al3i3=(st1ii(1).*vt1ii(2)-st1ii(2).*vt1ii(1))./dum;
al2i1=(al3i2.*al1i3-al3i3.*al1i2);
al2i2=(al3i3.*al1i1-al3i1.*al1i3);
al2i3=(al3i1.*al1i2-al3i2.*al1i1);

tli(1,1)=al1i1;
tli(1,2)=al1i2;
tli(1,3)=al1i3;
tli(2,1)=al2i1;
tli(2,2)=al2i2;
tli(2,3)=al2i3;
tli(3,1)=al3i1;
tli(3,2)=al3i2;
tli(3,3)=al3i3;

tli_orig(1:min(prod(tli_shape),numel(tli_orig)))=tli(1:min(prod(tli_shape),numel(tli_orig)));tli=tli_orig;
return;
end %subroutine cadli
%**********************************************************************

function [disbwx,blon,blat,wlon,wlat]=caddis(disbwx,blon,blat,wlon,wlat);

%*** GREAT CIRCLE GROUND DISTANCE BETWEEN B AND W IN KM: DISBWX,
%    GIVEN THE LONGITUDES AND LATITUDES OF B AND W



global bcom_1; if isempty(bcom_1), bcom_1=zeros(1,3510); end;
%% common c(3510);
%% common bcom_1(3510);

%*** INPUT FROM EXECUTIVE

% equivalence(bcom_1(0051),rearth) ::;

dum=sin(wlat).*sin(blat)+cos(wlat).*cos(blat).*cos(wlon-blon);
if(abs(dum) > 1.)
dum=(abs(1.).*sign(dum));
end;
disbwx=rearth.*acos(dum).*1.0e-3;


return;
end %subroutine caddis
%*******************************************************************

function [tge,blon,blat]=cadtge(tge,blon,blat);
%***  **************************************************************
%***  *
%***  * Calculates transformation matrix TGE from longitude and latitude
%***  *
%***  * Argument Output:
%***  *          TGE(3,3) =Transf. of geographic wrt earth coor
%***  * Argument Input:
%***  *          BLON =Missile longitude - rad
%***  *          BLAT =Missile latitude - rad
%***  *
%***  * MODIFICATION HISTORY
%***  * 960628 Created by Peter Zipfel
%***  *
%***  **************************************************************


tge_orig=tge;tge_shape=[3,3];tge=reshape([tge_orig(1:min(prod(tge_shape),numel(tge_orig))),zeros(1,max(0,prod(tge_shape)-numel(tge_orig)))],tge_shape);
global bcom_1; if isempty(bcom_1), bcom_1=zeros(1,3510); end;
%% common  c(3510);
%% common  bcom_1(3510);



slon=sin(blon);
clon=cos(blon);
slat=sin(blat);
clat=cos(blat);
tge(1,1)=-slat.*clon;
tge(1,2)=-slat.*slon;
tge(1,3)=clat;
tge(2,1)=-slon;
tge(2,2)=clon;
tge(2,3)=0.;
tge(3,1)=-clat.*clon;
tge(3,2)=-clat.*slon;
tge(3,3)=-slat;

tge_orig(1:min(prod(tge_shape),numel(tge_orig)))=tge(1:min(prod(tge_shape),numel(tge_orig)));tge=tge_orig;
return;
end %subroutine cadtge
%**********************************************************************

function [sbii,blon,blat,balt,alon0]=cadine(sbii,blon,blat,balt,alon0);

%*** CALCULATES INERTIAL POSITION VECTOR FROM LON,LAT,ALT



global bcom_1; if isempty(bcom_1), bcom_1=zeros(1,3510); end;
%% common c(3510);
%% common bcom_1(3510);



%*** INPUT FROM EXEC

% equivalence(bcom_1(0051),rearth) ::;
% equivalence(bcom_1(0058),weii3) ::;
% equivalence(bcom_1(2000),t) ::;

%*** CELESTIAL LONGITUDE

blam=blon-alon0+weii3.*t;

%*** POSITION VECTOR

brad=rearth+balt;
cblat=cos(blat);
sbii(1)=brad.*cblat.*cos(blam);
sbii(2)=brad.*cblat.*sin(blam);
sbii(3)=brad.*sin(blat);




return;
end %subroutine cadine
%**********************************************************************

function [tgi,alon,alat,alon0]=cadtgi(tgi,alon,alat,alon0);

%*** GEOGRAPHIC TO INERTIAL COOR. TRANSFORMATION MATRIX FROM LAT AND LON



tgi_orig=tgi;tgi_shape=[3,3];tgi=reshape([tgi_orig(1:min(prod(tgi_shape),numel(tgi_orig))),zeros(1,max(0,prod(tgi_shape)-numel(tgi_orig)))],tgi_shape);
global bcom_1; if isempty(bcom_1), bcom_1=zeros(1,3510); end;
%% common c(3510);
%% common bcom_1(3510);



%*** INPUT FROM EXEC

% equivalence(bcom_1(2000),t) ::;
% equivalence(bcom_1(0058),weii3) ::;

alamb=alon-alon0+weii3.*t;

tgi(1,3)=cos(alat);
tgi(3,3)=-sin(alat);
tgi(2,2)=cos(alamb);
tgi(2,1)=-sin(alamb);
tgi(1,1)=tgi(3,3).*tgi(2,2);
tgi(1,2)=-tgi(3,3).*tgi(2,1);
tgi(3,1)=-tgi(1,3).*tgi(2,2);
tgi(3,2)=tgi(1,3).*tgi(2,1);
tgi(2,3)=0.;



tgi_orig(1:min(prod(tgi_shape),numel(tgi_orig)))=tgi(1:min(prod(tgi_shape),numel(tgi_orig)));tgi=tgi_orig;
return;
end %subroutine cadtgi

%--- GETTING ENVIRONMENT VARIABLE VALUE

function [getenvqqresult,varname, value]=getenvqq(varname, value);
getenvqqresult=[];

;
%DEC$ ATTRIBUTES DEFAULT :: GETENVQQ


% , intent(in out)                         ::  4;
function [gresult]=g(varargin);
gresult=[];
persistent lenmlv ; 

;
value=repmat(' ',1,1);
if isempty(lenmlv), lenmlv=.*) varname; end;
end %function getenvqq

